#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <mkl_lapacke.h>
//#define iter_max n
//#define index_h(i,j) ((i)*iter_max+(j))
#define index_h(i,j) ((i) * (iter_max) + (j))
#define index_v(i,j) ((j) * (iter_max) + (i))
void givens(float a, float b, float &sn, float &cs, double &r)
{
    r = sqrt(a*a + b*b);
    cs = a/r;
    sn = b/r;
}
//! original update operations
void Update_tr(float *y, const int k,  float *Ht, const int iter_max, float *s)
{
    int i=0, j=0;
    //float *y = (float*) malloc((k+1)*sizeof(float));
    for (i=0; i<k; i++)  
        y[i] = s[i];

    // Back substituation, Ht is lower triangle matrix
    // solve y, where Ht*y = s
    for (i = 0; i < k; i++) {
        for (j=0; j < i; j++)
            y[i] -= Ht[index_h(i,j)] * y[j];
        y[i] /= Ht[index_h(i,i)];// the diagonal element
    }
}

void harm_Ritz_valvec(float* H, int n, float* wr, float *wi, float* vr, int ord[],int kR, int flag, int tid, float *rR); 
extern "C" void comp_eig(float *a, int N, float *wr, float *wi, float *vr, int ord[], int tid); 

extern "C" void  harm_Ritz_vec( int kR, float *H, int iter_max, int *ord, float *vr, int flag, int tid, float *rR) {

    float* wr = (float*)malloc( iter_max * sizeof(float));
    float* wi = (float*)malloc( iter_max * sizeof(float));
    //float* vr = (float*)malloc(iter_max  * iter_max  * sizeof(float));

    harm_Ritz_valvec(H, iter_max, wr, wi, vr, ord, kR, flag, tid, rR);
    /*
    for(int i=0;i<kR;i++){
        if(wi[ord[i]]==0.0){
            
        }
    }*/
    free(wi);
    free(wr);
/*
    printf("Eigenvalues:\n");
    for (int i = 0; i < n - 1; i++) {
        printf("%f ", lambda[i]);
    }
    printf("\n");

    printf("Eigenvectors:\n");
    for (int i = 0; i < n - 1; i++) {
        for (int j = 0; j < n - 1; j++) {
            printf("%f ", g[i * (n - 1) + j]);
        }
        printf("\n");
    }*/

    //free(lambda);
    //free(g);

    //return 0;
}
void harm_Ritz_valvec(float* H, int iter_max, float* wr, float *wi, float* vr, int ord[], int kR, int flag, int tid, float *rR) {
    int k = iter_max ;
    float* Hk = (float*)malloc(k * k * sizeof(float));
    float* Ht = (float*)malloc(k * k * sizeof(float));
    float* ek = (float*)calloc(k, sizeof(float));
    //float* sn = (float*)calloc(k, sizeof(float));
    //float* cs = (float*)calloc(k, sizeof(float));
    float* y = (float*)calloc(k, sizeof(float));
    if(kR<=0) return;
//    FILE *fp=fopen("output.txt","a");
    // Copy submatrix Ht(1:k, 1:k) to Hk
    int is,js; 
    float tmp_sum=0.0;
    float sn,cs;
  /*  if(tid==0){
        fprintf(fp,"\nH=");
        for(is=0;is<k;is++){
            for(js=0;js<k;js++){
                fprintf(fp,"%5.4e ",H[index_h(is,js)]);
             }
        fprintf(fp,"\n");
        }

        fprintf(fp,"\n");
    }*/
    //printf("Ht=\n ");
    for (int i = 0; i < k; i++) {
        for (int j = 0; j < k; j++) {
            Ht[index_h(j,i)] = H[index_h(i,j)];
            Hk[index_h(i,j)] = H[index_h(i,j)];
    //printf("%.2e ", Ht[index_h(j,i)]);
        }
    //printf("\n ");
    }

    ek[k - 1] = 1.0;
    double tmp_r;
    if(flag==0){
        for(int i=k-1;i>=1;i--){
            givens(Ht[index_h(i,i)],Ht[index_h(i-1,i)],sn,  cs, tmp_r);
            Ht[index_h(i,i)]=tmp_r; Ht[index_h(i-1,i)]=0.0;
            ek[i-1]=-sn*ek[i]; ek[i]=cs*ek[i];
            
            for (int j = 0; j<i ; j++) {
                tmp_r=cs*Ht[index_h(i,j)]+sn*Ht[index_h(i-1,j)];
                Ht[index_h(i-1,j)]=-sn*Ht[index_h(i,j)]+cs*Ht[index_h(i-1,j)];
                Ht[index_h(i,j)]=tmp_r;
            }
        }
    }else{
         for(int i=k-1;i>=kR+1;i--){
            givens(Ht[index_h(i,i)],Ht[index_h(i-1,i)],sn,  cs, tmp_r);
            Ht[index_h(i,i)]=tmp_r; Ht[index_h(i-1,i)]=0.0;
            ek[i-1]=-sn*ek[i]; ek[i]=cs*ek[i];
            
            for (int j = 0; j<i ; j++) {
                tmp_r=cs*Ht[index_h(i,j)]+sn*Ht[index_h(i-1,j)];
                Ht[index_h(i-1,j)]=-sn*Ht[index_h(i,j)]+cs*Ht[index_h(i-1,j)];
                Ht[index_h(i,j)]=tmp_r;
            }
        }   
        for(int i=kR;i>=1;i--){
        for(int ii=i-1;ii>=0;ii--){
            float tmp=-Ht[index_h(ii,i)]/Ht[index_h(i,i)];
            ek[ii]+=tmp*ek[i];
            for (int j = 0; j<=i ; j++) {
                Ht[index_h(ii,j)]+= tmp * Ht[index_h(i,j)];
            }
        }
        }
    }
    /*
    if(tid==0){
        fprintf(fp,"\nHt=");
        for(is=0;is<k;is++){
            for(js=0;js<k;js++){
                fprintf(fp,"%5.4e ",Ht[index_h(is,js)]);
             }
        fprintf(fp,"\n");
        }

        fprintf(fp,"\n");
    }*/

    Update_tr(y, k, Ht, iter_max, ek);
    /*if(tid==0){
            fprintf(fp,"\nek=");
        for(int i=0;i<k;i++){
            fprintf(fp,"%5.4e ",ek[i]);
        }
            fprintf(fp,"\n");
            fprintf(fp,"\ny=");
        for(int i=0;i<k;i++){
            fprintf(fp,"%5.4e ",y[i]);
        }
            fprintf(fp,"\n");
    }
    fclose(fp);*/
/*    if(tid==12)   {
        printf("\ny=\n ");
        for (int i = 0; i < k; i++) 
            printf("%.3e ",y[i]);
    }*/
    tmp_r=H[index_h(iter_max, iter_max-1)];
    for (int i = 0; i < iter_max; i++) rR[i]= -tmp_r*y[i];
    rR[iter_max]=1;
    tmp_r= H[index_h(k, k-1)]; tmp_r*=tmp_r;
    //if(tid==12) printf("\nH-1=%.3e,H=%.3e,tmp_r=%.3e\n",H[index_h(k-1, k-1)], H[index_h(k, k-1)],tmp_r);
    // Update Hk(:,k)
    for (int i = 0; i < k; i++) {
        Hk[index_h(i,k-1)] += tmp_r * y[i];
    }
    comp_eig(Hk, k, wr, wi, vr, ord, tid); 
    /*
 if(tid==12)   {
        printf("\neig= ");
        for (int i = 0; i < k; i++) 
            printf("%.3e ",wr[ord[i]]);
    }*/
/*        tmp_sum=0.0;
        for(is=0;is<k;is++){
            for(js=0;js<k;js++){
                tmp_sum+=vr[index_h(is,js)];
             }
        }
        printf("tid=%d,vr_sum=%e\n",tid,tmp_sum);
*/
    //fclose(fp);
    free(Hk);
    free(Ht);
    free(ek);
    free(y);
   /*
    free(Hk);
    free(ek);
    free(D);
    free(X);
    free(I);&*/
}


